#include "StdAfx.h"
#include "Frame.h"
#include "Line.h"
#include "Intersection.h"
#include "Util.h"




using namespace std;
using namespace cv;

#define PROB true
#define TAO 10
#define NEIGHBOR_THRESH 24
#define PI 3.1415926
#define VERTICAL 0
#define HORIZONTAL 1
#define WINDOW_SIZE 32

//Comparator used to sort lines.
struct linecompare {
	bool operator() (Line a, Line b) { 
		vector<Point> aPts = a.getPoints();
		vector<Point> bPts = b.getPoints();
		int aVal, bVal;

		if(a.isHorizontal()) {
			aVal = aPts[0].y;
			bVal = bPts[0].y;
			if((aPts[0].x == bPts[1].x && aPts[0].y == bPts[1].y) || (aPts[1].x == bPts[0].x && aPts[1].y == bPts[0].y)){
				aVal = aPts[0].x;
				bVal = bPts[0].x; 
			}

		}
		else {
			aVal = aPts[0].x;
			bVal = bPts[0].x;
			if((aPts[0].x == bPts[1].x && aPts[0].y == bPts[1].y) || (aPts[1].x == bPts[0].x && aPts[1].y == bPts[0].y)){
				aVal = aPts[0].y;
				bVal = bPts[0].y; 
			}
		}

		return aVal < bVal;
	}
} comparitor;

//comparator for intersections in a horizontal line
struct horizontalIntersectCompare {
	bool operator() (Intersection a, Intersection b) { 
		int aVal, bVal;
		aVal = a.intSecPt.x;
		bVal = b.intSecPt.x;
		return aVal < bVal;
	}
} hIntersectComparitor;

//comparator for intersections in a vertical line.
struct verticalIntersectCompare {
	bool operator() (Intersection a, Intersection b) { 
		int aVal, bVal;
		aVal = a.intSecPt.y;
		bVal = b.intSecPt.y;
		return aVal < bVal;
	}
} vIntersectComparitor;


//Frame constructor takes in the frame Mat.
Frame::Frame(Mat cvFrame) : _cvFrame(cvFrame), _cvOrigFrame(cvFrame)
{
	buildLineMask();
	detectAndFilterLines(_lines);
	//if more than 1 horizontal and 1 vertical line detected.
	if(verticalLines.size()>1  && horizontalLines.size()>1){
		sortLines();
		detectIntersections(_intersections);
		resizeLines();
		buildScoreMask();
		sort(segmentHorizontalLines.begin(), segmentHorizontalLines.end(), comparitor);
		sort(segmentVerticalLines.begin(), segmentVerticalLines.end(), comparitor);
	}

	//visualize the detected lines in frame when DEBUG flag is on.
	if(DEBUG){
		Mat m = _cvFrame.clone();
		drawFrame(m, true);
		imshow("Debug", m);
		cvWaitKey(0);
		//imwrite("resized.png", m);
	}
}

Frame::Frame()
{
}

void Frame::detectAndFilterLines(vector<Line>& lines) {
	detectLines(lines);
	//filterLines(lines);
	filterLinesVertical();
	filterLinesHorizontal();
	lines.clear();
	lines.insert(lines.end(), verticalLines.begin(), verticalLines.end());
	lines.insert(lines.end(), horizontalLines.begin(), horizontalLines.end());

	if(DEBUG){
		Mat m = _cvFrame.clone();
		drawFrame(m, true);
		imshow("Debug", m);
		cvWaitKey(0);
		//imwrite("filtered.png", m);
	}
}


//Detect lines on the court
void Frame::detectLines(vector<Line>& outputLines)
{
	//dilate canny edge _lineMask to make the lines wider.
	Mat element = getStructuringElement( MORPH_RECT, Size(3, 3));
	dilate(_lineMask, _lineMask, element);
	//store it into _scoreMat to be used by ModelMatch to score homographies by checking the model lines mapped on to _scoreMat.
	_scoreMat = _lineMask.clone();
	
	//detect lines
	vector<Vec4i> lines;
	HoughLinesP(_lineMask, lines, 1, CV_PI/180, _cvFrame.rows*0.1, 10);
	
	//If debug is on, visualize the detected lines.
	if(DEBUG){
		cvNamedWindow("Debug", CV_WINDOW_AUTOSIZE);
		imshow("Debug", _lineMask);
		cvWaitKey(0);
		Mat m = _cvFrame.clone();
		for(size_t i = 0; i < MIN(lines.size(), 300); i++)
		{
			Vec4i line = lines[i];
			Point p1(line[0], line[1]);
			Point p2(line[2], line[3]);
			Line tempLine(p1,p2);
			if(tempLine.isVertical()){
				tempLine.drawLine(m, Scalar(255,0,0));
			}
			else if(tempLine.isHorizontal()){
				tempLine.drawLine(m, Scalar(0,0,255));
			}
		}
		imshow("Debug", m);
		//imwrite("hough.png", m);
		cvWaitKey(0);
	}


	vector<Line> tempHorizontalLines;
	vector<Line> tempVerticalLines;


	for(size_t i = 0; i < MIN(lines.size(), 300); i++){
		Vec4i line = lines[i];
		Point p1(line[0], line[1]);
		Point p2(line[2], line[3]);
		Line tempLine(p1,p2);
		tempLine.createDirVec();
		

		if(tempLine.isHorizontal()) {		
			//Keep horizontal lines with length more than %5 of the width of frame
			if(abs(p1.x - p2.x) > _cvFrame.cols*0.05){
				//take out horizontal lines that on the border of the frame (top and bottom 5%)
				if(! ((_cvFrame.rows - p1.y) < (_cvFrame.rows*0.05) || (p1.y < _cvFrame.rows*0.05))){
					tempHorizontalLines.push_back(tempLine);
				}
			}
		}
		else if(tempLine.isVertical()) {
			//take out vertical lines that on the border of the frame (left and right 5%)
			if(! ((_cvFrame.cols - p1.x) < (_cvFrame.cols*0.05) || (p1.x < _cvFrame.cols*0.05))){
				tempVerticalLines.push_back(tempLine);
			}
				
		}
	}

	for(size_t i = 0; i < tempHorizontalLines.size(); i++){
		Line tempLine = tempHorizontalLines[i];
		outputLines.push_back(tempLine);
		horizontalLines.push_back(tempLine);
	}
	for(size_t i = 0; i < tempVerticalLines.size(); i++){
		Line tempLine = tempVerticalLines[i];
		outputLines.push_back(tempLine);
		verticalLines.push_back(tempLine);
	}
	
	//visualize the remaining detected lines.
	if(DEBUG){
		Mat m = _cvFrame.clone();
		drawFrame(m, true);
		imshow("Debug", m);
		cvWaitKey(0);
		//imwrite("lines.png", m);
	}
}


//NOT USED. filter lines that are similar to the lines already existing
void Frame::filterLines(vector<Line>& lines)
{
	vector<Line> output;
	verticalLines.clear();
	horizontalLines.clear();

	for(size_t i = 0; i < lines.size(); i++) {
		Line l = lines[i];

		if(l.isHorizontal()) {
			bool similar = similarHorizontalLineExist(horizontalLines, l);

			if(!similar) {
				horizontalLines.push_back(l);
			}
		}

		else if(l.isVertical()) {
			bool similar = similarVerticalLineExist(verticalLines, l);
			
			if(!similar) {
				verticalLines.push_back(l);
			}
		}
	}

	output.insert(output.end(), verticalLines.begin(), verticalLines.end());
	output.insert(output.end(), horizontalLines.begin(), horizontalLines.end());
	
	lines = output;
}



//filter vertical lines that are similar based on line's X location at Y = _cvFrame.rows/2
void Frame::filterLinesVertical(){
	
	vector<Line> filterdVerticalLines;
	Mat verticalXVals = Mat::zeros(1, verticalLines.size(), CV_32F);
	Mat angleVals = Mat::zeros(1, verticalLines.size(), CV_32F);

	//compute each line's x location at y= _cvFrame.rows/2
	for(int i=0; i<verticalLines.size(); i++){
		Line l = verticalLines[i];
		Point testP1 = l.getPoints()[0];
		Point testP2 = l.getPoints()[1];
		//float xVal1 = ((testP1.x-testP2.x) * (_cvFrame.rows/2 - testP2.y)*1.0) / (testP1.y - testP2.y) + testP2.x;
		float xVal1 = ((testP1.x-testP2.x) * (_cvFrame.rows/2 - testP2.y)*1.0) / (testP1.y - testP2.y) + testP2.x;
		verticalXVals.at<float>(0, i) = xVal1;
		angleVals.at<float>(0, i) = l.getAngle(); 
	}

	//calculating midpoints histogram
	double min, max;
	Point minLoc, maxLoc;
	minMaxLoc(verticalXVals, &min, &max, &minLoc, &maxLoc, Mat());
	int histSize = (max-min)/4;
	if(histSize ==0){
		return;
	}
	float range[] = { (float)min, (float)max } ;
	const float* histRange = { range };
	bool uniform = true; bool accumulate = false;
	Mat verticalHist;
	calcHist( &verticalXVals, 1, 0, Mat(), verticalHist, 1, &histSize, &histRange, uniform, accumulate );
	double binRange = (max-min)/histSize;

	//finding local maxima in the histograms.
	vector<int> maximums = findLocalMaximums(verticalHist, VERTICAL, binRange);

	//one vertical line is obtained from each spike/maximum in verticalHist.
	for(int i=0; i< maximums.size(); i++){
		double val = maximums[i] *4 + min;
		vector<Line> tempLines;

		//choose the lines that are close to the maximum x location.
		for(int j =0; j< verticalXVals.cols; j++){
			double diff = abs(verticalXVals.at<float>(0, j) - val);
			if(diff <= binRange*2){
				tempLines.push_back(verticalLines[j]);
			}
		}
		if(tempLines.size()<1) continue;

		//choose one single line that is mapped to the line on the court the best.
		double bestScore = INT_MAX;
		double bestIndex =0;
		for(int j=0; j<tempLines.size(); j++){
			Line l = tempLines[j];
			double length = l.getLength();
			double score = scoreVerticalLine(l);
			if(score < bestScore){
				bestScore = score;
				bestIndex = j;
			}
		}
		tempLines[bestIndex].extendLine(_cvFrame.rows, _cvFrame.cols);
		filterdVerticalLines.push_back(tempLines[bestIndex]);
	}
	verticalLines.clear();
	verticalLines.insert(verticalLines.end(), filterdVerticalLines.begin(), filterdVerticalLines.end());
}


//filter horizontal lines that are similar based on line's Y location at X = _cvFrame.cols/2
//exactly the same approach as the vertical method.
void Frame::filterLinesHorizontal(){
	vector<Line> filteredHorizontalLines;

	Mat horizontalYVals = Mat::zeros(1, horizontalLines.size(), CV_32F);
	for(int i=0; i<horizontalLines.size(); i++){
		Line l = horizontalLines[i];
		Point testP1 = l.getPoints()[0];
		Point testP2 = l.getPoints()[1];
		float yVal = ((testP1.y-testP2.y) * (_cvFrame.cols/2 - testP2.x)*1.0) / (testP1.x - testP2.x) + testP2.y;
		horizontalYVals.at<float>(0, i) = yVal;
	}
	double min, max;
	Point minLoc, maxLoc;
	minMaxLoc(horizontalYVals, &min, &max, &minLoc, &maxLoc, Mat());
	int histSize = (max-min)/4;
	if(histSize ==0){
		return;
	}

	float range[] = { (float)min, (float)max } ;
	const float* histRange = { range };
	bool uniform = true; bool accumulate = false;
	Mat horizontalHist;
	calcHist( &horizontalYVals, 1, 0, Mat(), horizontalHist, 1, &histSize, &histRange, uniform, accumulate );
	double binRange = (max-min)/histSize;
	
	//finding local maxium 
	vector<int> maximums = findLocalMaximums(horizontalHist, HORIZONTAL, binRange);


	for(int i=0; i< maximums.size(); i++){
		double val = maximums[i] *4 + min;
		vector<Line> tempLines;
		for(int j =0; j< horizontalYVals.cols; j++){
			double diff = abs(horizontalYVals.at<float>(0, j) - val);
			if(diff <= binRange*2){
				tempLines.push_back(horizontalLines[j]);
			}
		}
		if(tempLines.size()<1) continue;

		double bestScore = INT_MAX;
		double bestIndex =0;
		for(int j=0; j<tempLines.size(); j++){
			Line l = tempLines[j];
			double length = l.getLength();
			double score = scoreHorizontalLine(l);
			if(score < bestScore){
				bestScore = score;
				bestIndex = j;
			}
		}
		tempLines[bestIndex].extendLine(_cvFrame.rows, _cvFrame.cols);
		filteredHorizontalLines.push_back(tempLines[bestIndex]);
	}

	horizontalLines.clear();
	horizontalLines.insert(horizontalLines.end(), filteredHorizontalLines.begin(), filteredHorizontalLines.end());
}

//find the maxima in a given histogram
vector<int> Frame::findLocalMaximums(Mat histogram, int orientation, double binRange){
	vector<int> maximums;
	
	int zeroThreshold;

	if(orientation == VERTICAL){
		zeroThreshold = _cvFrame.cols/(25*binRange);
	}
	else{
		zeroThreshold = _cvFrame.rows/(25*binRange);
	}
	vector<int> index, value;

	for(int i=0; i< histogram.rows; i++){ 
		if(histogram.at<float>(i, 0) != 0){
			index.push_back(i);
			value.push_back(histogram.at<float>(i, 0));
		}
	}

	//segments the histogram into multiple parts that are separated by many 0s and pick a maximum from each segment
	int current;
	int baseIndex =index[0];
	vector<int> segmentValues;
	segmentValues.push_back(value[0]);
	for(int i=1; i<index.size(); i++){
		current = index[i];
		if((current - baseIndex) > zeroThreshold){
			int maxVal =0;
			int maxIndex =0;

			for(int j=0; j < segmentValues.size(); j++){
				if(!(segmentValues[j]<maxVal)){
					maxIndex = j;
					maxVal =  segmentValues[j];
				}
			}
			maximums.push_back(maxIndex + baseIndex);
			segmentValues.clear();
			baseIndex =current;
			segmentValues.push_back(value[i]);
		}
		else{
			segmentValues.push_back(value[i]);
		}
	}

	//get the maximum from the last segment
	int maxVal =0;
	int maxIndex =0;
	for(int j=0; j < segmentValues.size(); j++){
		if(segmentValues[j]>maxVal){
			maxIndex = j;
			maxVal =  segmentValues[j];
		}
	}
	maximums.push_back(maxIndex + baseIndex);
	return maximums;
}

vector<Line> Frame::getSegmentHorLines(){
	return segmentHorizontalLines;
}
vector<Line> Frame::getSegmentVerLines(){
	return segmentVerticalLines;
}

//resizing vertical and horizontal lines. Also create line segments based on intermediate intersection points.
void Frame::resizeLines() {
	int linesCount = verticalLines.size();
	
	for(size_t i = 0; i < linesCount; i++) {
		Line& line = verticalLines[i];
		line.reFitLine();
		vector<Intersection> inters  = line.getIntersectionPts();
		if (inters.size()==0) continue;

		//sort the intersection points belongs to this vertical line.
		sort(inters.begin(), inters.end(), vIntersectComparitor);
		
		//creating vertical line segments with intermediate intersectoin points.
		Point startPoint = inters[0].intSecPt;
		Point endPoint;
		for (int i=1; i<inters.size(); i++){
			endPoint = inters[i].intSecPt;
			Line tempLine(startPoint, endPoint);
			if(tempLine.isVertical()) segmentVerticalLines.push_back(tempLine);
			startPoint = endPoint;
		}
	}

	//creating horizontal line segments with intermediate intersectoin points.
	linesCount = horizontalLines.size();
	for(size_t i = 0; i < linesCount; i++) {
		Line& line = horizontalLines[i];
		line.reFitLine();
		vector<Intersection> inters  = line.getIntersectionPts();
		if (inters.size()==0) continue;

		sort(inters.begin(), inters.end(), hIntersectComparitor);
		
		Point startPoint = inters[0].intSecPt;
		Point endPoint;
		for (int i=1; i<inters.size(); i++){
			endPoint = inters[i].intSecPt;
			Line tempLine(startPoint, endPoint);
			if(tempLine.isHorizontal()) segmentHorizontalLines.push_back(tempLine);
			startPoint = endPoint;
		}
	}
}

//Sorts lines using order Left to Right and then Up and Down.
void Frame::sortLines() {
	sort(verticalLines.begin(), verticalLines.end(), comparitor);
	sort(horizontalLines.begin(), horizontalLines.end(), comparitor);
}


// NOT USED. decide if a horizontal line is similar to other vertical lines.
bool Frame::similarHorizontalLineExist(vector<Line> lines, Line line) {

	vector<Point> testPts = line.getPoints();
	Point testP1 = testPts[0];
	Point testP2 = testPts[1];

	for(size_t i = 0; i < lines.size(); i++) {
		Line l = lines[i];

		vector<Point> comparepts = l.getPoints();
		Point cmpP1 = comparepts[0];
		Point cmpP2 = comparepts[1];

		double dist11 = abs(testP1.y - cmpP1.y);
		double dist12 = abs(testP1.y - cmpP2.y);
		double dist21 = abs(testP2.y - cmpP1.y);
		double dist22 = abs(testP2.y - cmpP2.y);
	
		double minVal = min(min(dist11, dist12), min(dist21, dist22));
		if(minVal <= (_cvFrame.rows*0.05)) {
			return true;
		}
	}
	return false;
}


// NOT USED. decide if a vertical line is similar to other vertical lines.
bool Frame::similarVerticalLineExist(vector<Line> lines, Line line) {
	vector<Point> testPts = line.getPoints();
	Point testP1 = testPts[0];
	Point testP2 = testPts[1];
	double testAngle = line.getAngle();

	for(size_t i = 0; i < lines.size(); i++) {
		Line l = lines[i];

		vector<Point> comparepts = l.getPoints();
		Point cmpP1 = comparepts[0];
		Point cmpP2 = comparepts[1];

		//double dist11 = abs(testP1.x - cmpP1.x);
		//double dist12 = abs(testP1.x - cmpP2.x);
		//double dist21 = abs(testP2.x - cmpP1.x);
		//double dist22 = abs(testP2.x - cmpP2.x);
	
		//double minVal = min(min(dist11, dist12), min(dist21, dist22));
		
		double xVal1 = ((testP1.x-testP2.x) * (_cvFrame.rows/2 - testP2.y)*1.0) / (testP1.y - testP2.y) + testP2.x;
		double xVal2 = ((cmpP1.x-cmpP2.x) * (_cvFrame.rows/2 - cmpP2.y)*1.0) / (cmpP1.y - cmpP2.y) + cmpP2.x;
		double minVal = abs(xVal1 - xVal2);

		
		//double minVal = (testP1.x + testP2.x)/2.0 + (cmpP1.x + cmpP2.x)/2.0;

		double cmpAngle = l.getAngle();
		double angDiff = abs(cmpAngle - testAngle);

		//Point intersection = l.findIntersection(line);
		//if(intersection.x>0 && intersection.x<_cvFrame.cols && intersection.y>0 && intersection.y<_cvFrame.rows){
		//	if(abs(testP1.x-intersection.x) < _cvFrame.cols*0.1 && abs(testP2.x - intersection.x) < _cvFrame.cols*0.1){
		//		return true;
		//	}
		//}
		if(minVal <= (_cvFrame.cols*0.03) && angDiff <= 30) {
			return true;
		}
	}
	return false;
}


//find intersection points of all vertical lines and horizontal lines.
void Frame::detectIntersections(vector<Intersection> &intersectionPts) {

	for(size_t hor = 0; hor < horizontalLines.size(); hor++) {
		Line& hLine = horizontalLines[hor];

		for(size_t ver = 0; ver < verticalLines.size(); ver++) {
			Line& vLine = verticalLines[ver];
		 
			Point intersection = hLine.findIntersection(vLine);
			
			Intersection intsec(intersection);
			intersectionPts.push_back(intsec);

			hLine.addIntersection(intsec);
			vLine.addIntersection(intsec);
		}
	}
}

vector<Line> Frame::getHorLines() {
	return horizontalLines;
}

vector<Line> Frame::getVerLines() {
	return verticalLines;
}

vector<Intersection> Frame::getIntSecs() {
	return _intersections;
}

//NOT USED.Finds the 4 bounding lines of a court.
//Why do I do this? Just sort them and select 0 index and last index.
//Changed to use the Sorted Lists. Keeping old code here just incase something goes wrong later.
vector<Line> Frame::findBoundingLines() 
{
	vector<Line> lines(4);
	/*Line maxHor = findMaxHorzLine(_lines);
	Line minHor = findMinHorzLine(_lines);

	Line maxVer = findMaxVertLine(_lines);
	Line minVer = findMinVertLine(_lines);*/

	Line maxHor = horizontalLines[horizontalLines.size()-1];
	Line minHor = horizontalLines[0];
	Line maxVer = verticalLines[verticalLines.size()-1];
	Line minVer = verticalLines[0];

	lines[0] = minHor;
	lines[1] = maxHor;
	lines[2] = minVer;
	lines[3] = maxVer;

	return lines;
}
//NOT USED
Line Frame::findMinVertLine(vector<Line> lines) {
	Line minLine;
	int minX = std::numeric_limits<int>::max();

	for(size_t i = 0; i < lines.size(); i++) {
		Line currLine = lines[i];

		if(currLine.isVertical()) {

			vector<Point> points = currLine.getPoints();
			Point p1 = points[0];
			Point p2 = points[1];

			int currMin = min(p1.x, p2.x);
			if(currMin < minX) {
				minLine = currLine;
				minX = currMin;
			}
		}
	}
	return minLine;
}
//NOT USED
Line Frame::findMaxVertLine(vector<Line> lines) {
	Line maxLine;
	int maxX = std::numeric_limits<int>::min();

	for(size_t i = 0; i < lines.size(); i++) {
		Line currLine = lines[i];

		if(currLine.isVertical()) {
			vector<Point> points = currLine.getPoints();
			Point p1 = points[0];
			Point p2 = points[1];

			int currMax = max(p1.x, p2.x);
			if(currMax > maxX) {
				maxLine = currLine;
				maxX = currMax;
			}
		}
	}
	return maxLine;
}
//NOT USED
Line Frame::findMinHorzLine(vector<Line> lines) {
	Line minLine;
	int minX = std::numeric_limits<int>::max();

	for(size_t i = 0; i < lines.size(); i++) {
		Line currLine = lines[i];

		if(currLine.isHorizontal()) {

			vector<Point> points = currLine.getPoints();
			Point p1 = points[0];
			Point p2 = points[1];

			int currMin = min(p1.y, p2.y);
			if(currMin < minX) {
				minLine = currLine;
				minX = currMin;
			}
		}
	}
	return minLine;
}
//NOT USED
Line Frame::findMaxHorzLine(vector<Line> lines) {
	Line maxLine;
	int maxX = std::numeric_limits<int>::min();

	for(size_t i = 0; i < lines.size(); i++) {
		Line currLine = lines[i];

		if(currLine.isHorizontal()) {
			vector<Point> points = currLine.getPoints();
			Point p1 = points[0];
			Point p2 = points[1];

			int currMax = max(p1.y, p2.y);
			if(currMax > maxX) {
				maxLine = currLine;
				maxX = currMax;
			}
		}
	}
	return maxLine;
}


//process the image so it can be used by detectLines to detect lines.
void Frame::buildLineMask()
{
	
	cvtColor(_cvFrame, _lineMask, CV_BGR2GRAY);
	
	//thresholding the gray scale image. The threshold is actually very important. Some threhold works well for some video, but not others
	Scalar meanScalar = mean(_lineMask);
	//double mean = meanScalar[0]*1.75-20;
	double mean = meanScalar[0]*1.75;
	//double mean = 160;
	
	//make sure threshold is within a range.
	if(mean >184){
		mean = 184;
	}
	if(mean < 128){
		mean = 128;
	}
	//threshold values to zero if they are below and keep their own value if they are above.
	threshold(_lineMask, _lineMask, mean, 0, CV_THRESH_TOZERO);
	cout<<mean<<endl;
	//equalize the grayscale image.
	equalizeHist( _lineMask, _lineMask );
	//threshold again with much higher threshold and set values below threshold to zero above to 255.
	threshold(_lineMask, _lineMask, 220, 255, CV_THRESH_TOZERO);
	// visualize the thresholded grayscale image.
	if(DEBUG){
		cvNamedWindow("Debug", CV_WINDOW_AUTOSIZE);
		imshow("Debug", _lineMask);
		cvWaitKey(0);
		//imwrite("threshold.png", _lineMask);
	}


	//remove large white regions.
	//see "Robust Camera Calibration for Sport Videos using Court Models"
    Mat processingMat = Mat::zeros(_lineMask.size(), _lineMask.type());
    for(int i = 0; i < _cvFrame.rows; i++)
    {
        int upIndex = i - TAO;
        int downIndex = i + TAO;

        for(int j = 0; j < _cvFrame.cols; j++)
        {
            int leftIndex = j - TAO;
            int rightIndex = j + TAO;
            uchar pixelVal = _lineMask.at<uchar>(i,j);

            if(leftIndex>0 && (pixelVal - _lineMask.at<uchar>(i,leftIndex)) > NEIGHBOR_THRESH){
                    if(rightIndex < _cvFrame.cols && (pixelVal - _lineMask.at<uchar>(i,rightIndex)) > NEIGHBOR_THRESH){
                            processingMat.at<uchar>(i,j)= 255;
                    }
            }
            else if(upIndex > 0 && (pixelVal - _lineMask.at<uchar>(upIndex,j)) > NEIGHBOR_THRESH){
                    if(downIndex < _cvFrame.rows && (pixelVal - _lineMask.at<uchar>(downIndex,j))> NEIGHBOR_THRESH){
                            processingMat.at<uchar>(i,j)= 255;
                    }
            }
        }
    }
    _lineMask = processingMat.clone();
	//detect edges
	Mat edges;
	Canny(_lineMask, edges, 80, 250, 3);
	
	//visualize the image after canny edge detection
	if(DEBUG){
		cvNamedWindow("Debug", CV_WINDOW_AUTOSIZE);
		imshow("Debug", edges);
		cvWaitKey(0);
		//imwrite("canny.png", edges);
	}

	
	Mat verGradient, horGraidient;
	Sobel(edges, verGradient, CV_32F, 0, 1);
	Sobel(edges, horGraidient, CV_32F, 1, 0);
	Mat gradAngleMat(verGradient.rows, verGradient.cols, CV_32F);
	Mat gradMagMat(verGradient.rows, verGradient.cols, CV_32F);
	//get gradient magnitude and angle values.
	for(int r =0; r<verGradient.rows; r++){
		for(int c=0; c< horGraidient.cols; c++){
			float dx = horGraidient.at<float>(r, c);
			float dy = verGradient.at<float>(r, c);
			float mag = sqrt( pow(dx,2) + pow(dy,2)); 
			gradMagMat.at<float>(r, c) = mag;
			float angle = atan2(dy, dx) * 180/PI;
			if(angle < 0){
				angle = angle + 360.0;
			}
			gradAngleMat.at<float>(r, c) = angle;
		}
	}

	//filter edge pixels based on gradient values.
	for(int r =0; r<(verGradient.rows-WINDOW_SIZE); r+=WINDOW_SIZE){
		for(int c=0; c< (horGraidient.cols-WINDOW_SIZE); c+=WINDOW_SIZE){
			
			//get a submat from the gradient angle mat.
			Mat angleVals(gradAngleMat,  Range(r, r+WINDOW_SIZE), Range(c, c+WINDOW_SIZE));
			
			//compute the angle histogram.
			int histSize = 120;
			float range[] = { (float)0, (float)360 } ;
			const float* histRange = { range };
			bool uniform = true; bool accumulate = false;
			Mat histogram;
			calcHist( &angleVals, 1, 0, Mat(), histogram, 1, &histSize, &histRange, uniform, accumulate );
			
			//get the first dominant gradient angle other than 0.
			histogram.at<float>(0, 0) = 0;
			double min, max;
			Point minLoc, maxLoc;
			minMaxLoc(histogram, &min, &max, &minLoc, &maxLoc, Mat());
			Scalar countScalar = sum(histogram);
			int totalCount = countScalar[0];

			//get the second dominant graident angle.
			double max1;
			histogram.at<float>(maxLoc.y, maxLoc.x) = 0;
			minMaxLoc(histogram, &min, &max1, &minLoc, &maxLoc, Mat());
			
			//if the sum of the two dominat gradients are less than 40% of the patch, then this patch is set to black.
			if(totalCount ==0) continue;
			if(((max+max1) /totalCount)< 0.4){
				for (int j = r; j<r+WINDOW_SIZE; j++){
					for(int i =c; i<c+WINDOW_SIZE; i++){
						edges.at<uchar>(j, i) =0;
					}
				}
			}
		}
	}

	//visualize final _lineMask.
	if(DEBUG){
		cvNamedWindow("Debug", CV_WINDOW_AUTOSIZE);
		imshow("Debug", edges);
		cvWaitKey(0);
		//imwrite("gradient.png", edges);
	}
	_lineMask = edges.clone();
}

//draw vertical and horizontal lines and intersection onto the frame.
void Frame::drawFrame(cv::Mat& mat, bool withLines) {
	//mat = _cvFrame.clone();
	if(withLines) {
		for(size_t v = 0; v < verticalLines.size(); v++) {
			Line vl = verticalLines[v];
			vl.drawLine(mat, Scalar(255,0,0));
		}

		for(size_t h = 0; h < horizontalLines.size(); h++) {
			Line hl = horizontalLines[h];
			hl.drawLine(mat, Scalar(0,0,255));
		}

		for(size_t i = 0; i < _intersections.size(); i++) {
			Intersection intSec = _intersections[i];
			circle(mat, intSec.intSecPt, 4, Scalar(0,0,255), 5);
		}
	}
}

//Draw detected lines onto score mask.
void Frame::buildScoreMask(){
	for(size_t v = 0; v < verticalLines.size(); v++) {
		Line vl = verticalLines[v];
		vl.drawLine(_scoreMat, Scalar(255,0,0));
	}

	for(size_t h = 0; h < horizontalLines.size(); h++) {
		Line hl = horizontalLines[h];
		hl.drawLine(_scoreMat, Scalar(255,0,0));
	}
}

//check how well a given vertical line is mapped onto the edges in the frame.
double Frame::scoreVerticalLine(Line l){
	double score =0;
	Point p1 = l.getPoints()[0];
	Point p2 = l.getPoints()[1];
	int yStart =0; 
	int yEnd = _cvFrame.rows;

	//for each pixel compute the x location given y and check if the location is a white pixel.
	for(int y=yStart; y<yEnd; y++){
		float x = ((p1.x-p2.x) * (y - p2.y)*1.0) / (p1.y - p2.y) + p2.x;
		if(goodMatch(x, y)){
			score--;
		}
		else{
			score+=0;
		}
	}
	return score;
}

//check how well a given horizontal line is mapped onto the edges in the frame.
double Frame::scoreHorizontalLine(Line l){
	double score =0;
	Point p1 = l.getPoints()[0];
	Point p2 = l.getPoints()[1];
	int xStart = 0;
	int xEnd = _cvFrame.cols;
	
	//for each pixel compute the x location given y and check if the location is a white pixel.
	for(int x=xStart; x<xEnd; x++){
		float y = ((p1.y-p2.y) * (x - p2.x)*1.0) / (p1.x - p2.x) + p2.y;
		if(goodMatch(x, y)){
			score--;
		}
		else{
			score+=0;
		}
	}
	return score;
}

//check if the col and row location lies on a white pixel.
bool Frame::goodMatch(int col, int row){
	int xWindowSize = 1;
	int yWindowSize = 1;

	for(int r=-yWindowSize; r<=yWindowSize; r++){
		for(int c=-xWindowSize; c<=xWindowSize; c++){
			if((col+c)>0 && (col+c)<_cvFrame.cols && (r+row)>0 && (r+row)<_cvFrame.rows){
				uchar value = _lineMask.at<uchar>((row+r),(col+c));
				if(value>0){
					return true;
				}
			}
		}
	}
	return false;
}


//Computing homograpphy that relates the current frame and the given previous frame.
bool Frame::homographyBetweenFrame(Mat homography, Mat prevFrame){
	Mat prevGrey, currentGrey;

	//convert both frames to greyscale.
	cvtColor(prevFrame, prevGrey, CV_BGR2GRAY);
	cvtColor(_cvFrame, currentGrey, CV_BGR2GRAY);

	//detect and extract features from keypoints
	int minHessian = 300;
	SurfFeatureDetector detector( minHessian );
	vector<KeyPoint> keypoints_prev, keypoints_curr;
	detector.detect( prevGrey, keypoints_prev );
	detector.detect( currentGrey, keypoints_curr);
	SurfDescriptorExtractor extractor;
	Mat descriptors_prev, descriptors_curr;
	extractor.compute( prevGrey, keypoints_prev, descriptors_prev );
	extractor.compute( currentGrey, keypoints_curr, descriptors_curr );

	//match features in both frames.
	FlannBasedMatcher matcher;
	vector< DMatch > matches;
	matcher.match( descriptors_prev, descriptors_curr, matches );

	double max_dist = 0; double min_dist = 100;
	//-- Quick calculation of max and min distances between keypoints
	for( int i = 0; i < descriptors_prev.rows; i++ ){ 
		double dist = matches[i].distance;
		if( dist < min_dist ) min_dist = dist;
		if( dist > max_dist ) max_dist = dist;
	}
	
	//if the displacement is small return the identity matrix.
	if(min_dist < 0.001 ){
		homography = Mat::eye(3, 3, CV_64F);	
		return false;
	}
	printf("min_distance: %f \n", (min_dist));

	//filter to get "good" matches (i.e. whose distance is less than 3*min_dist )
	std::vector< DMatch > good_matches;
	for( int i = 0; i < descriptors_prev.rows; i++ ){ 
		if( matches[i].distance < 3*min_dist ){
			good_matches.push_back( matches[i]); 
		}
	}
	//if not enough good matches, then return the identity matrix.
	if(good_matches.size()<4){
		homography = Mat::eye(3, 3, CV_64F);	
		return false;
	}

	//use the good matches to compute the homography between the frames.
	std::vector<Point2f> prev;
	std::vector<Point2f> curr;
	for( int i = 0; i < good_matches.size(); i++ ){
		//-- Get the keypoints from the good matches
		prev.push_back( keypoints_prev[ good_matches[i].queryIdx ].pt );
		curr.push_back( keypoints_curr[ good_matches[i].trainIdx ].pt );
	}
	homography = findHomography( prev, curr, CV_RANSAC );
	

	////for visualizing the matches of two frames.
	//if(DEBUG){
		//Mat img_matches;
		//drawMatches( prevGrey, keypoints_prev, currentGrey, keypoints_curr,
		//		   good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
		//		   vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
	
		////-- Get the corners from the image_1 ( the object to be "detected" )
		//std::vector<Point2f> obj_corners(4);
		//obj_corners[0] = cvPoint(0,0); obj_corners[1] = cvPoint( prevGrey.cols, 0 );
		//obj_corners[2] = cvPoint( prevGrey.cols, prevGrey.rows ); obj_corners[3] = cvPoint( 0, prevGrey.rows );
		//std::vector<Point2f> scene_corners(4);

		//perspectiveTransform( obj_corners, scene_corners, homography);

		////-- Draw lines between the corners (the mapped object in the scene - image_2 )
		//line( img_matches, scene_corners[0] + Point2f( prevGrey.cols, 0), scene_corners[1] + Point2f( prevGrey.cols, 0), Scalar(0, 255, 0), 4 );
		//line( img_matches, scene_corners[1] + Point2f( prevGrey.cols, 0), scene_corners[2] + Point2f( prevGrey.cols, 0), Scalar( 0, 255, 0), 4 );
		//line( img_matches, scene_corners[2] + Point2f( prevGrey.cols, 0), scene_corners[3] + Point2f( prevGrey.cols, 0), Scalar( 0, 255, 0), 4 );
		//line( img_matches, scene_corners[3] + Point2f( prevGrey.cols, 0), scene_corners[0] + Point2f( prevGrey.cols, 0), Scalar( 0, 255, 0), 4 );

		////-- Show detected matches
		//imshow( "Good Matches & Object detection", img_matches );

		//waitKey(0);
	//}
	return true;
}



Frame::~Frame(void)
{
}
